#include "stdint.h"
#include "DevicesPID.h"


void vPIDDeInit(PIDType *ptypePID, int32_t kp, int32_t ki, int32_t kd, int32_t errMax, int32_t integralMax, int32_t integralMin, int32_t outMax, int32_t outMin)
{
    ptypePID->out = ptypePID->errLast = ptypePID->integral = 0;
    ptypePID->kp            = kp;
    ptypePID->ki            = ki;
    ptypePID->kd            = kd;

    ptypePID->errMax        = errMax;
    ptypePID->integralMax   = integralMax;
    ptypePID->integralMin   = integralMin;
    ptypePID->outMax        = outMax;
    ptypePID->outMin        = outMin;
}

/*
Return Value:   PID调节后的输出值
Parameters:     fSet: 设定值;
                fNow: 当前值;
Description:    位置型PID调节算法
*/
int32_t iPIDLocation(PIDType *ptypePID, int32_t set, int32_t now)
{
    int32_t error = set - now;

    if((ptypePID->errMax < error) || (error < -ptypePID->errMax))
        ptypePID->integral = 0;
    else
        ptypePID->integral += ptypePID->ki * error;

    /* 限定积分上、下限 */
    ptypePID->integral = (ptypePID->integral > ptypePID->integralMax) ? ptypePID->integralMax :
                        ((ptypePID->integral < ptypePID->integralMin) ? ptypePID->integralMin : ptypePID->integral);

    /* PID 运算 */
    ptypePID->out = ptypePID->kp * error + ptypePID->integral + ptypePID->kd * (error - ptypePID->errLast);

    ptypePID->errLast = error;

    /* 限定输出上、下限 */
    ptypePID->out = (ptypePID->out > ptypePID->outMax) ? ptypePID->outMax :
                   ((ptypePID->out < ptypePID->outMin) ? ptypePID->outMin : ptypePID->out);

    return ptypePID->out;
}
